// Reads and parses messages from HIL interface program on serial0
// everything here should only be executed if (VX_SIM == VX_SIM_HIL) 
// switch for that lies at the call of the function in the main loop


#if VX_SIM == VX_SIM_HIL 

#include <util/crc16.h>

void vx_HILCheckMessages() {
  // Read entire serial buffer

  while(Serial.available() > 0) {
    vx_HIL_recv_c = Serial.read();


    // Check for new message header ($) -> start buffering
    if(vx_HIL_recv_c == '$') {
      vx_HIL_recv_buffering = true; // Turn on buffering
      vx_HIL_recvBuffer = ""; // Reset receiving buffer
    }

    // Check if current mode is set to buffer (incoming message) -> buffer
    if(vx_HIL_recv_buffering) {
      vx_HIL_recvBuffer.concat(vx_HIL_recv_c);

      // Check for end of message (\n) -> Reset buffering & Parse message
      if(vx_HIL_recv_c == '\n') {
        vx_HIL_recv_buffering = false; // Turn off buffering

        // Extract message header
        vx_HIL_recvMsgHeader = vx_HIL_recvBuffer.substring(1,2);

        // Checksum
        String vx_HIL_recvBuffer_nochecksum = vx_HIL_recvBuffer.substring(0, (vx_HIL_recvBuffer.length()-4));
        vx_HIL_Checksum(vx_HIL_recvBuffer_nochecksum);                                                                //FIXME copy and rename routine
        //Serial.println("DEBUG: calculated checksum for vx_HIL_recvBuffer = " + vx_HIL_crc_s);

        if(vx_HIL_crc_s != vx_HIL_recvBuffer.substring((vx_HIL_recvBuffer.length()-3),(vx_HIL_recvBuffer.length()-1))) { //FIXME rename in copied routine
          //vx_teleSendWarning(VX_TELE_WARNING_CRCERR); //FIXME Warning MEssage
        }
        else {  
          if(vx_HIL_recvMsgHeader.equals("g")) {
            vx_HILParseGpsMessage();              
          }
          else if(vx_HIL_recvMsgHeader.equals("a")) {
            vx_HILParseAttitudeMessage();
          }
        }

      }
    }
  }
}

void vx_HIL_Checksum(String HIL_msg) {
  uint8_t crc = 0;
  uint8_t i;

  // Calculate checksum
  for(i=0; i<HIL_msg.length(); i++) {
    crc = _crc_ibutton_update(crc, (uint8_t)HIL_msg[i]);
  }

  // Convert to a nice string
  //vx_HIL_crc_s = "";
  if(crc < 16) {
    //vx_HIL_crc_s.concat('0');
    vx_HIL_crc_s = "0";
  }
  else {
    vx_HIL_crc_s = "";
  }

  vx_HIL_crc_s.concat(String(crc, HEX));
  vx_HIL_crc_s.toUpperCase();
}

void vx_HILParseGpsMessage() {
  //Serial.println("DEBUG: HILParseGpsMessage() called.");

  int current_pos = 2;
  int max_var_length = 10;
  int next_var = 0;
  int next_var_pos = 0;

  vx_g_hil_lat = vx_HIL_ParseGetNextLong(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_HIL_recvBuffer.indexOf(',', next_var_pos);
  vx_g_hil_lon = vx_HIL_ParseGetNextLong(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_HIL_recvBuffer.indexOf(',', next_var_pos);
  vx_g_hil_alt = vx_HIL_ParseGetNextLong(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_HIL_recvBuffer.indexOf(',', next_var_pos);
  vx_g_hil_hdg = vx_HIL_ParseGetNextLong(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_HIL_recvBuffer.indexOf(',', next_var_pos);
  vx_g_hil_gpsspd = vx_HIL_ParseGetNextLong(current_pos, max_var_length, &next_var_pos);

//  Serial.println("DEBUG: HIL GPS Message parsed.");
//  Serial.print(vx_g_hil_lat);
//  Serial.print(" ");
//  Serial.print(vx_g_hil_lon);
//  Serial.print(" ");
//  Serial.print(vx_g_hil_alt);
//  Serial.print(" ");
//  Serial.print(vx_g_hil_hdg);
//  Serial.print(" ");
//  Serial.print(vx_g_hil_gpsspd);
//  Serial.println("");
}




void vx_HILParseAttitudeMessage() {  
  //Serial.println("DEBUG: vx_HILParseAttitudeMessage() called.");

  int current_pos = 2;
  int max_var_length = 10;
  int next_var = 0;
  int next_var_pos = 0;

  vx_g_hil_roll = vx_HIL_ParseGetNextLong(current_pos, max_var_length, &next_var_pos)/1000.; //vx_g_hil_roll [rad] msg is as rad * 1000
  current_pos = 1 + vx_HIL_recvBuffer.indexOf(',', next_var_pos);
  vx_g_hil_nick = vx_HIL_ParseGetNextLong(current_pos, max_var_length, &next_var_pos)/1000.; //vx_g_hil_nick [rad] msg is as rad * 1000

//  Serial.println("DEBUG: HIL Attitude Message parsed.");
//  Serial.print(vx_g_hil_roll);
//  Serial.print(" ");
//  Serial.print(vx_g_hil_nick);
//  Serial.print(" ");  

}

long vx_HIL_ParseGetNextLong(int start_pos, int max_length, int *next_var_pos) { 
  char c = vx_HIL_recvBuffer[start_pos];
  int i_valbuf = 0;
  char c_valbuf[24];

  // Get buffer, stop on separators or when exceeding value max (character) length
  while(i_valbuf < max_length && c != '\n' && c != ',' && c != '*' && c != '\0') {
    c_valbuf[i_valbuf] = c;
    i_valbuf++;
    c = vx_HIL_recvBuffer[start_pos + i_valbuf];
  }

  // Save the last position
  if(next_var_pos != NULL) {
    *next_var_pos = start_pos + i_valbuf;
  }

  // Terminate buffer
  c_valbuf[i_valbuf] = '\0';

  // Convert to int
  return atol(c_valbuf);
}


// Send functions

void vx_HIL_SerialOutCRC() {
  // Sends current message WITH CRC
  vx_HIL_Checksum(vx_HIL_sendBuffer); // calculate crc 

  vx_HIL_sendBuffer.concat('*'); // checksum separator
  vx_HIL_sendBuffer.concat(vx_HIL_crc_s); // checksum
  vx_HIL_sendBuffer.concat('\n'); // End of Message

  Serial.print(vx_HIL_sendBuffer);

  vx_HIL_sendBuffer = ""; // Reset vx_sendBuffer
}


void vx_HILSendFlightControl() {
  vx_HIL_sendBuffer = "$c"; 

  if(vx_g_flightphase == 1)
  {
    vx_HIL_sendBuffer.concat(String(vx_g_rcin_nick()*5));
    vx_HIL_sendBuffer.concat(','); // Separator
    vx_HIL_sendBuffer.concat(String(vx_g_rcin_roll()*5));
    
    //vx_HIL_sendBuffer.concat(String((int)(APM_RC.InputCh(1)-1500)));
    //vx_HIL_sendBuffer.concat(','); // Separator
    //vx_HIL_sendBuffer.concat(String((int)(APM_RC.InputCh(0)-1500)));
  }
  else
  {
    vx_HIL_sendBuffer.concat(String((int)vx_g_stabilizeout_nick));
    vx_HIL_sendBuffer.concat(','); // Separator
    vx_HIL_sendBuffer.concat(String((int)vx_g_stabilizeout_roll));
  }



  vx_HIL_SerialOutCRC();
}


#endif





